#!/usr/bin/env python3
# -*- coding:utf-8 -*-

import rclpy
from rclpy.node import Node
from cv_bridge import CvBridge
from sensor_msgs.msg import PointCloud2, Image
import time
import os
import numpy as np
import ros2_numpy
import cv2
from spirems import Publisher, Subscriber, def_msg, cvimg2sms, BaseNode, get_extra_args
from scipy.spatial.transform import Rotation as R
import threading
from typing import Union
import argparse

"""
依赖项安装：
pip install spirems ros2-numpy
"""

DEFAULT_IP = "127.0.0.1"


class ROS2ThreeLidar2SMSNode(Node, threading.Thread, BaseNode):
    def __init__(
        self,
        job_name: str,
        ip: str = '127.0.0.1',
        port: int = 9094,
        param_dict_or_file: Union[dict, str] = None,
        sms_shutdown: bool = True,
        **kwargs
    ):
        Node.__init__(self, self.__class__.__name__)
        threading.Thread.__init__(self)
        sms_shutdown = True if sms_shutdown in ['True', 'true', '1', True] else False
        BaseNode.__init__(
            self,
            self.__class__.__name__,
            job_name,
            ip=ip,
            port=port,
            param_dict_or_file=param_dict_or_file,
            sms_shutdown=sms_shutdown,
            **kwargs
        )

        self.lidar_front_origin = self.get_param("lidar_front_origin", {"xyz": [0.738, -0.006, -0.002], "rpy": [3.1415926, -3.1415926/2, 0]})
        self.lidar_left_origin = self.get_param("lidar_left_origin", {"xyz": [-0.151, 0.2385, 0.2436], "rpy":[3.1415926, -3.1415926/2, 3.1415926*2/3]})
        self.lidar_right_origin = self.get_param("lidar_right_origin", {"xyz": [-0.151, -0.2508, 0.2436], "rpy": [3.1415926, -3.1415926/2, -3.1415926*2/3]})
        self.camera_fl_origin = self.get_param("camera_fl_origin", {"xyz": [0.7042, 0.2285, 0.045], "rpy": [-3.1415926/2, 0, -3.1415926/2]})
        self.camera_fr_origin = self.get_param("camera_fr_origin", {"xyz": [0.7042, -0.2285, 0.045], "rpy": [-3.1415926/2, 0, -3.1415926/2]})
        self.camera_cl_origin = self.get_param("camera_cl_origin", {"xyz": [0.1845, 0.3577, 0.047], "rpy": [-3.1415926/2, 0, -3.1415926/6]})
        self.camera_cr_origin = self.get_param("camera_cr_origin", {"xyz": [0.1845, -0.3577, 0.047], "rpy": [-3.1415926/2, 0, -3.1415926*5/6]})
        self.camera_rl_origin = self.get_param("camera_rl_origin", {"xyz": [-0.3955, 0.2648, 0.287], "rpy": [-3.1415926/2, 0, 3.1415926/6]})
        self.camera_rr_origin = self.get_param("camera_rr_origin", {"xyz": [-0.3955, -0.2648, 0.287], "rpy": [-3.1415926/2, 0, 3.1415926*5/6]})
        self.camera_r_origin = self.get_param("camera_r_origin", {"xyz": [-0.3998, 0, 0.287], "rpy": [-3.1415926/2, 0, 3.1415926/2]})

        self.camera_fl_calib = self.get_param("camera_fl_calib", {"camera_matrix": [956.98, 0, 337.24, 0, 957.69, 179.01, 0, 0, 1], "distortion_coefficients": [0.0, 0.0, 0.0, 0.0, 0.0]})
        self.camera_fr_calib = self.get_param("camera_fr_calib", {"camera_matrix": [961.05, 0, 367.15, 0, 958.66, 227.94, 0, 0, 1], "distortion_coefficients": [0.0, 0.0, 0.0, 0.0, 0.0]})
        self.camera_cl_calib = self.get_param("camera_cl_calib", {"camera_matrix": [901.03, 0, 324.89, 0, 897.77, 163.30, 0, 0, 1], "distortion_coefficients": [0.0, 0.0, 0.0, 0.0, 0.0]})
        self.camera_cr_calib = self.get_param("camera_cr_calib", {"camera_matrix": [919.25, 0, 339.56, 0, 917.51, 178.72, 0, 0, 1], "distortion_coefficients": [0.0, 0.0, 0.0, 0.0, 0.0]})
        self.camera_rl_calib = self.get_param("camera_rl_calib", {"camera_matrix": [935.00, 0, 328.26, 0, 932.37, 164.18, 0, 0, 1], "distortion_coefficients": [0.0, 0.0, 0.0, 0.0, 0.0]})
        self.camera_rr_calib = self.get_param("camera_rr_calib", {"camera_matrix": [928.65, 0, 296.91, 0, 927.10, 176.79, 0, 0, 1], "distortion_coefficients": [0.0, 0.0, 0.0, 0.0, 0.0]})
        self.camera_r_calib = self.get_param("camera_r_calib", {"camera_matrix": [945.16, 0, 355.02, 0, 949.04, 202.67, 0, 0, 1], "distortion_coefficients": [0.0, 0.0, 0.0, 0.0, 0.0]})
        self.params_help()

        self.sensor_lidar_front_sub = self.create_subscription(
            PointCloud2,
            "/sensor/lidar_front/points",
            self.sensor_lidar_front_callback,
            10
        )
        self.sensor_lidar_left_sub = self.create_subscription(
            PointCloud2,
            "/sensor/lidar_left/points",
            self.sensor_lidar_left_callback,
            10
        )
        self.sensor_lidar_right_sub = self.create_subscription(
            PointCloud2,
            "/sensor/lidar_right/points",
            self.sensor_lidar_right_callback,
            10
        )
        self.sensor_camera_fl_sub = self.create_subscription(
            Image,
            "/sensor/camera/camera_fl/sxcam_raw",
            self.sensor_camera_fl_callback,
            10
        )
        self.sensor_camera_fr_sub = self.create_subscription(
            Image,
            "/sensor/camera/camera_fr/sxcam_raw",
            self.sensor_camera_fr_callback,
            10
        )
        self.sensor_camera_cl_sub = self.create_subscription(
            Image,
            "/sensor/camera/camera_cl/sxcam_raw",
            self.sensor_camera_cl_callback,
            10
        )
        self.sensor_camera_cr_sub = self.create_subscription(
            Image,
            "/sensor/camera/camera_cr/sxcam_raw",
            self.sensor_camera_cr_callback,
            10
        )
        self.sensor_camera_rl_sub = self.create_subscription(
            Image,
            "/sensor/camera/camera_rl/sxcam_raw",
            self.sensor_camera_rl_callback,
            10
        )
        self.sensor_camera_rr_sub = self.create_subscription(
            Image,
            "/sensor/camera/camera_rr/sxcam_raw",
            self.sensor_camera_rr_callback,
            10
        )
        self.sensor_camera_r_sub = self.create_subscription(
            Image,
            "/sensor/camera/camera_r/sxcam_raw",
            self.sensor_camera_r_callback,
            10
        )
        self.lidar_front_on = False
        self.lidar_left_on = False
        self.lidar_right_on = False

        self.sms_lidar_front_pub = Publisher("/flyeagle/lidar_front", "memory_msgs::PointCloud", ip=DEFAULT_IP)
        self.sms_lidar_left_pub = Publisher("/flyeagle/lidar_left", "memory_msgs::PointCloud", ip=DEFAULT_IP)
        self.sms_lidar_right_pub = Publisher("/flyeagle/lidar_right", "memory_msgs::PointCloud", ip=DEFAULT_IP)

        self.sms_camera_fl_pub = Publisher("/flyeagle/camera_fl", "memory_msgs::RawImage", ip=DEFAULT_IP)
        self.sms_camera_fr_pub = Publisher("/flyeagle/camera_fr", "memory_msgs::RawImage", ip=DEFAULT_IP)
        self.sms_camera_cl_pub = Publisher("/flyeagle/camera_cl", "memory_msgs::RawImage", ip=DEFAULT_IP)
        self.sms_camera_cr_pub = Publisher("/flyeagle/camera_cr", "memory_msgs::RawImage", ip=DEFAULT_IP)
        self.sms_camera_rl_pub = Publisher("/flyeagle/camera_rl", "memory_msgs::RawImage", ip=DEFAULT_IP)
        self.sms_camera_rr_pub = Publisher("/flyeagle/camera_rr", "memory_msgs::RawImage", ip=DEFAULT_IP)
        self.sms_camera_r_pub = Publisher("/flyeagle/camera_r", "memory_msgs::RawImage", ip=DEFAULT_IP)

        self.sms_calib_fl_pub = Publisher("/flyeagle/camera_fl/calibration_info", 'sensor_msgs::CameraCalibration')
        self.sms_calib_fr_pub = Publisher("/flyeagle/camera_fr/calibration_info", 'sensor_msgs::CameraCalibration')
        self.sms_calib_cl_pub = Publisher("/flyeagle/camera_cl/calibration_info", 'sensor_msgs::CameraCalibration')
        self.sms_calib_cr_pub = Publisher("/flyeagle/camera_cr/calibration_info", 'sensor_msgs::CameraCalibration')
        self.sms_calib_rl_pub = Publisher("/flyeagle/camera_rl/calibration_info", 'sensor_msgs::CameraCalibration')
        self.sms_calib_rr_pub = Publisher("/flyeagle/camera_rr/calibration_info", 'sensor_msgs::CameraCalibration')
        self.sms_calib_r_pub = Publisher("/flyeagle/camera_r/calibration_info", 'sensor_msgs::CameraCalibration')

        self.p_front = self.lidar_front_origin["xyz"]
        r_front = R.from_euler('xyz', self.lidar_front_origin["rpy"])
        self.q_front = r_front.as_quat()
        self.p_left = self.lidar_left_origin["xyz"]
        r_left = R.from_euler('xyz', self.lidar_left_origin["rpy"])
        self.q_left = r_left.as_quat()
        self.p_right = self.lidar_right_origin["xyz"]
        r_right = R.from_euler('xyz', self.lidar_right_origin["rpy"])
        self.q_right = r_right.as_quat()

        self.p_fl = self.camera_fl_origin["xyz"]
        r_fl = R.from_euler('xyz', self.camera_fl_origin["rpy"])
        self.q_fl = r_fl.as_quat()
        self.p_fr = self.camera_fr_origin["xyz"]
        r_fr = R.from_euler('xyz', self.camera_fr_origin["rpy"])
        self.q_fr = r_fr.as_quat()
        self.p_cl = self.camera_cl_origin["xyz"]
        r_cl = R.from_euler('xyz', self.camera_cl_origin["rpy"])
        self.q_cl = r_cl.as_quat()
        self.p_cr = self.camera_cr_origin["xyz"]
        r_cr = R.from_euler('xyz', self.camera_cr_origin["rpy"])
        self.q_cr = r_cr.as_quat()
        self.p_rl = self.camera_rl_origin["xyz"]
        r_rl = R.from_euler('xyz', self.camera_rl_origin["rpy"])
        self.q_rl = r_rl.as_quat()
        self.p_rr = self.camera_rr_origin["xyz"]
        r_rr = R.from_euler('xyz', self.camera_rr_origin["rpy"])
        self.q_rr = r_rr.as_quat()

        self.p_r = self.camera_r_origin["xyz"]
        r_r = R.from_euler('xyz', self.camera_r_origin["rpy"])
        self.q_r = r_r.as_quat()

        self.sms_trans_front_pub = Publisher("/flyeagle/lidar_trans", "geometry_msgs::FrameTransforms", ip=DEFAULT_IP)
        self.sms_base_pub = Publisher("/flyeagle/base_link", "geometry_msgs::PoseInFrame", ip=DEFAULT_IP)
        self.bridge = CvBridge()

    def sensor_camera_fl_callback(self, msg):
        ros_time = msg.header.stamp
        cv_image = self.bridge.imgmsg_to_cv2(msg, "bgr8")
        sms_msg = self.sms_camera_fl_pub.cvimg2sms_mem(cv_image, frame_id="camera_fl", timestamp=ros_time.sec + ros_time.nanosec / 1e9)
        self.sms_camera_fl_pub.publish(sms_msg)

        msg_calb = def_msg('sensor_msgs::CameraCalibration')
        msg_calb['frame_id'] = "camera_fl"
        msg_calb['width'] = cv_image.shape[1]
        msg_calb['height'] = cv_image.shape[0]
        msg_calb['K'] = self.camera_fl_calib["camera_matrix"]
        msg_calb['D'] = self.camera_fl_calib["distortion_coefficients"]
        K = self.camera_fl_calib["camera_matrix"]
        msg_calb['P'] = [K[0], K[1], K[2], 0, K[3], K[4], K[5], 0, K[6], K[7], K[8], 0]
        self.sms_calib_fl_pub.publish(msg_calb)
    
    def sensor_camera_fr_callback(self, msg):
        ros_time = msg.header.stamp
        cv_image = self.bridge.imgmsg_to_cv2(msg, "bgr8")
        sms_msg = self.sms_camera_fr_pub.cvimg2sms_mem(cv_image, frame_id="camera_fr", timestamp=ros_time.sec + ros_time.nanosec / 1e9)
        self.sms_camera_fr_pub.publish(sms_msg)

        msg_calb = def_msg('sensor_msgs::CameraCalibration')
        msg_calb['frame_id'] = "camera_fr"
        msg_calb['width'] = cv_image.shape[1]
        msg_calb['height'] = cv_image.shape[0]
        msg_calb['K'] = self.camera_fr_calib["camera_matrix"]
        msg_calb['D'] = self.camera_fr_calib["distortion_coefficients"]
        K = self.camera_fr_calib["camera_matrix"]
        msg_calb['P'] = [K[0], K[1], K[2], 0, K[3], K[4], K[5], 0, K[6], K[7], K[8], 0]
        self.sms_calib_fr_pub.publish(msg_calb)
    
    def sensor_camera_cl_callback(self, msg):
        ros_time = msg.header.stamp
        cv_image = self.bridge.imgmsg_to_cv2(msg, "bgr8")
        sms_msg = self.sms_camera_cl_pub.cvimg2sms_mem(cv_image, frame_id="camera_cl", timestamp=ros_time.sec + ros_time.nanosec / 1e9)
        self.sms_camera_cl_pub.publish(sms_msg)

        msg_calb = def_msg('sensor_msgs::CameraCalibration')
        msg_calb['frame_id'] = "camera_cl"
        msg_calb['width'] = cv_image.shape[1]
        msg_calb['height'] = cv_image.shape[0]
        msg_calb['K'] = self.camera_cl_calib["camera_matrix"]
        msg_calb['D'] = self.camera_cl_calib["distortion_coefficients"]
        K = self.camera_cl_calib["camera_matrix"]
        msg_calb['P'] = [K[0], K[1], K[2], 0, K[3], K[4], K[5], 0, K[6], K[7], K[8], 0]
        self.sms_calib_cl_pub.publish(msg_calb)
    
    def sensor_camera_cr_callback(self, msg):
        ros_time = msg.header.stamp
        cv_image = self.bridge.imgmsg_to_cv2(msg, "bgr8")
        sms_msg = self.sms_camera_cr_pub.cvimg2sms_mem(cv_image, frame_id="camera_cr", timestamp=ros_time.sec + ros_time.nanosec / 1e9)
        self.sms_camera_cr_pub.publish(sms_msg)

        msg_calb = def_msg('sensor_msgs::CameraCalibration')
        msg_calb['frame_id'] = "camera_cr"
        msg_calb['width'] = cv_image.shape[1]
        msg_calb['height'] = cv_image.shape[0]
        msg_calb['K'] = self.camera_cr_calib["camera_matrix"]
        msg_calb['D'] = self.camera_cr_calib["distortion_coefficients"]
        K = self.camera_cr_calib["camera_matrix"]
        msg_calb['P'] = [K[0], K[1], K[2], 0, K[3], K[4], K[5], 0, K[6], K[7], K[8], 0]
        self.sms_calib_cr_pub.publish(msg_calb)
    
    def sensor_camera_rl_callback(self, msg):
        ros_time = msg.header.stamp
        cv_image = self.bridge.imgmsg_to_cv2(msg, "bgr8")
        sms_msg = self.sms_camera_rl_pub.cvimg2sms_mem(cv_image, frame_id="camera_rl", timestamp=ros_time.sec + ros_time.nanosec / 1e9)
        self.sms_camera_rl_pub.publish(sms_msg)

        msg_calb = def_msg('sensor_msgs::CameraCalibration')
        msg_calb['frame_id'] = "camera_rl"
        msg_calb['width'] = cv_image.shape[1]
        msg_calb['height'] = cv_image.shape[0]
        msg_calb['K'] = self.camera_rl_calib["camera_matrix"]
        msg_calb['D'] = self.camera_rl_calib["distortion_coefficients"]
        K = self.camera_rl_calib["camera_matrix"]
        msg_calb['P'] = [K[0], K[1], K[2], 0, K[3], K[4], K[5], 0, K[6], K[7], K[8], 0]
        self.sms_calib_rl_pub.publish(msg_calb)
    
    def sensor_camera_rr_callback(self, msg):
        ros_time = msg.header.stamp
        cv_image = self.bridge.imgmsg_to_cv2(msg, "bgr8")
        sms_msg = self.sms_camera_rr_pub.cvimg2sms_mem(cv_image, frame_id="camera_rr", timestamp=ros_time.sec + ros_time.nanosec / 1e9)
        self.sms_camera_rr_pub.publish(sms_msg)

        msg_calb = def_msg('sensor_msgs::CameraCalibration')
        msg_calb['frame_id'] = "camera_rr"
        msg_calb['width'] = cv_image.shape[1]
        msg_calb['height'] = cv_image.shape[0]
        msg_calb['K'] = self.camera_rr_calib["camera_matrix"]
        msg_calb['D'] = self.camera_rr_calib["distortion_coefficients"]
        K = self.camera_rr_calib["camera_matrix"]
        msg_calb['P'] = [K[0], K[1], K[2], 0, K[3], K[4], K[5], 0, K[6], K[7], K[8], 0]
        self.sms_calib_rr_pub.publish(msg_calb)
    
    def sensor_camera_r_callback(self, msg):
        ros_time = msg.header.stamp
        cv_image = self.bridge.imgmsg_to_cv2(msg, "bgr8")
        sms_msg = self.sms_camera_r_pub.cvimg2sms_mem(cv_image, frame_id="camera_r", timestamp=ros_time.sec + ros_time.nanosec / 1e9)
        self.sms_camera_r_pub.publish(sms_msg)

        msg_calb = def_msg('sensor_msgs::CameraCalibration')
        msg_calb['frame_id'] = "camera_r"
        msg_calb['width'] = cv_image.shape[1]
        msg_calb['height'] = cv_image.shape[0]
        msg_calb['K'] = self.camera_r_calib["camera_matrix"]
        msg_calb['D'] = self.camera_r_calib["distortion_coefficients"]
        K = self.camera_r_calib["camera_matrix"]
        msg_calb['P'] = [K[0], K[1], K[2], 0, K[3], K[4], K[5], 0, K[6], K[7], K[8], 0]
        self.sms_calib_r_pub.publish(msg_calb)

    def sensor_lidar_front_callback(self, msg):
        # t1 = time.time()
        cloud_arr = np.frombuffer(msg.data, np.float32).reshape(-1, 5)
        pcd_ = cloud_arr.copy(order='C')  # ros2_numpy.numpify(msg)['xyz']
        # 重新解析int32部分数据, 若雷达是FK24则不需要
        # int32_bytes = pcd_[:, [4, 5]].tobytes()
        # int32_data_tof = np.frombuffer(int32_bytes, dtype=np.int32).reshape(-1, 2).astype(np.float32)
        # pcd_[:, [4, 5]] = int32_data_tof
        # print(pcd_[:, 4].astype(np.int32)[:100])
        ros_time = msg.header.stamp
        sms_msg = self.sms_lidar_front_pub.pcl2sms_mem(
            pcd_, 
            ["x", "y", "z", "timestamp", "intensity"], 
            frame_id="lidar_front", 
            timestamp=ros_time.sec + ros_time.nanosec / 1e9
        )
        """
        sms_msg["pose"] = {}
        sms_msg["pose"]["position"] = {"x": 2.199, "y": 0.053, "z": 0.744}
        rot = R.from_euler('xyz', [-0.144, -1.550, -2.993])
        quaternion = rot.as_quat()
        sms_msg["pose"]["orientation"] = {"x": quaternion[0], "y": quaternion[1], "z": quaternion[2], "w": quaternion[3]}
        """
        # sms_msg['t1'] = time.time()
        self.sms_lidar_front_pub.publish(sms_msg)
        # print("dt:", time.time() - t1)
        if not self.lidar_front_on:
            self.lidar_front_on = True
            print("  FRONT LIDAR ON")

        trans_msg = def_msg("geometry_msgs::FrameTransforms")
        trans_msg["frame_id"] = "lidar_trans"
        trans_msg["transforms"] = [
            {
                "timestamp": time.time(),
                "parent_frame_id": "base_link",
                "child_frame_id": "lidar_front",
                "translation": {
                    "x": self.p_front[0],
                    "y": self.p_front[1],
                    "z": self.p_front[2]
                },
                "rotation": {
                    "x": self.q_front[0],
                    "y": self.q_front[1],
                    "z": self.q_front[2],
                    "w": self.q_front[3]
                }
            },
            {
                "timestamp": time.time(),
                "parent_frame_id": "base_link",
                "child_frame_id": "lidar_left",
                "translation": {
                    "x": self.p_left[0],
                    "y": self.p_left[1],
                    "z": self.p_left[2]
                },
                "rotation": {
                    "x": self.q_left[0],
                    "y": self.q_left[1],
                    "z": self.q_left[2],
                    "w": self.q_left[3]
                }
            },
            {
                "timestamp": time.time(),
                "parent_frame_id": "base_link",
                "child_frame_id": "lidar_right",
                "translation": {
                    "x": self.p_right[0],
                    "y": self.p_right[1],
                    "z": self.p_right[2]
                },
                "rotation": {
                    "x": self.q_right[0],
                    "y": self.q_right[1],
                    "z": self.q_right[2],
                    "w": self.q_right[3]
                }
            },
            {
                "timestamp": time.time(),
                "parent_frame_id": "base_link",
                "child_frame_id": "camera_fl",
                "translation": {
                    "x": self.p_fl[0],
                    "y": self.p_fl[1],
                    "z": self.p_fl[2]
                },
                "rotation": {
                    "x": self.q_fl[0],
                    "y": self.q_fl[1],
                    "z": self.q_fl[2],
                    "w": self.q_fl[3]
                }
            },
            {
                "timestamp": time.time(),
                "parent_frame_id": "base_link",
                "child_frame_id": "camera_fr",
                "translation": {
                    "x": self.p_fr[0],
                    "y": self.p_fr[1],
                    "z": self.p_fr[2]
                },
                "rotation": {
                    "x": self.q_fr[0],
                    "y": self.q_fr[1],
                    "z": self.q_fr[2],
                    "w": self.q_fr[3]
                }
            },
            {
                "timestamp": time.time(),
                "parent_frame_id": "base_link",
                "child_frame_id": "camera_cl",
                "translation": {
                    "x": self.p_cl[0],
                    "y": self.p_cl[1],
                    "z": self.p_cl[2]
                },
                "rotation": {
                    "x": self.q_cl[0],
                    "y": self.q_cl[1],
                    "z": self.q_cl[2],
                    "w": self.q_cl[3]
                }
            },
            {
                "timestamp": time.time(),
                "parent_frame_id": "base_link",
                "child_frame_id": "camera_cr",
                "translation": {
                    "x": self.p_cr[0],
                    "y": self.p_cr[1],
                    "z": self.p_cr[2]
                },
                "rotation": {
                    "x": self.q_cr[0],
                    "y": self.q_cr[1],
                    "z": self.q_cr[2],
                    "w": self.q_cr[3]
                }
            },
            {
                "timestamp": time.time(),
                "parent_frame_id": "base_link",
                "child_frame_id": "camera_rl",
                "translation": {
                    "x": self.p_rl[0],
                    "y": self.p_rl[1],
                    "z": self.p_rl[2]
                },
                "rotation": {
                    "x": self.q_rl[0],
                    "y": self.q_rl[1],
                    "z": self.q_rl[2],
                    "w": self.q_rl[3]
                }
            },
            {
                "timestamp": time.time(),
                "parent_frame_id": "base_link",
                "child_frame_id": "camera_rr",
                "translation": {
                    "x": self.p_rr[0],
                    "y": self.p_rr[1],
                    "z": self.p_rr[2]
                },
                "rotation": {
                    "x": self.q_rr[0],
                    "y": self.q_rr[1],
                    "z": self.q_rr[2],
                    "w": self.q_rr[3]
                }
            },
            {
                "timestamp": time.time(),
                "parent_frame_id": "base_link",
                "child_frame_id": "camera_r",
                "translation": {
                    "x": self.p_r[0],
                    "y": self.p_r[1],
                    "z": self.p_r[2]
                },
                "rotation": {
                    "x": self.q_r[0],
                    "y": self.q_r[1],
                    "z": self.q_r[2],
                    "w": self.q_r[3]
                }
            }
        ]
        self.sms_trans_front_pub.publish(trans_msg)
        base_msg = def_msg("geometry_msgs::PoseInFrame")
        base_msg["frame_id"] = "base_link"
        self.sms_base_pub.publish(base_msg)

    def sensor_lidar_left_callback(self, msg):
        cloud_arr = np.frombuffer(msg.data, np.float32).reshape(-1, 5)
        pcd_ = cloud_arr.copy(order='C')  # ros2_numpy.numpify(msg)['xyz']
        # int32_bytes = pcd_[:, [4, 5]].tobytes()
        # int32_data_tof = np.frombuffer(int32_bytes, dtype=np.int32).reshape(-1, 2).astype(np.float32)
        # pcd_[:, [4, 5]] = int32_data_tof
        ros_time = msg.header.stamp
        sms_msg = self.sms_lidar_left_pub.pcl2sms_mem(
            pcd_, 
            ["x", "y", "z", "timestamp", "intensity"], 
            frame_id="lidar_left", 
            timestamp=ros_time.sec + ros_time.nanosec / 1e9
        )
        """
        sms_msg["pose"] = {}
        sms_msg["pose"]["position"] = {"x": -0.151, "y": -0.2385, "z": -0.2436}
        rot = R.from_euler('xyz', [0, 1.5708, -2.0944])
        quaternion = rot.as_quat()
        sms_msg["pose"]["orientation"] = {"x": quaternion[0], "y": quaternion[1], "z": quaternion[2], "w": quaternion[3]}
        """
        self.sms_lidar_left_pub.publish(sms_msg)
        if not self.lidar_left_on:
            self.lidar_left_on = True
            print("  LEFT LIDAR ON")

    def sensor_lidar_right_callback(self, msg):
        cloud_arr = np.frombuffer(msg.data, np.float32).reshape(-1, 5)
        pcd_ = cloud_arr.copy(order='C')  # ros2_numpy.numpify(msg)['xyz']
        # int32_bytes = pcd_[:, [4, 5]].tobytes()
        # int32_data_tof = np.frombuffer(int32_bytes, dtype=np.int32).reshape(-1, 2).astype(np.float32)
        # pcd_[:, [4, 5]] = int32_data_tof
        ros_time = msg.header.stamp
        sms_msg = self.sms_lidar_right_pub.pcl2sms_mem(
            pcd_, 
            ["x", "y", "z", "timestamp", "intensity"], 
            frame_id="lidar_right", 
            timestamp=ros_time.sec + ros_time.nanosec / 1e9
        )
        """
        sms_msg["pose"] = {}
        sms_msg["pose"]["position"] = {"x": -0.151, "y": 0.2508, "z": -0.2436}
        rot = R.from_euler('xyz', [0, 1.5708, 2.0944])
        quaternion = rot.as_quat()
        sms_msg["pose"]["orientation"] = {"x": quaternion[0], "y": quaternion[1], "z": quaternion[2], "w": quaternion[3]}
        """
        self.sms_lidar_right_pub.publish(sms_msg)
        if not self.lidar_right_on:
            self.lidar_right_on = True
            print("  RIGHT LIDAR ON")


if __name__ == '__main__':
    parser = argparse.ArgumentParser()
    parser.add_argument(
        '--config',
        type=str,
        default='default_params.json',
        help='SpireCV2 Config (.json)')
    parser.add_argument(
        '--job-name',
        type=str,
        default='live',
        help='SpireCV Job Name')
    parser.add_argument(
        '--ip',
        type=str,
        default='127.0.0.1',
        help='SpireMS Core IP')
    parser.add_argument(
        '--port',
        type=int,
        default=9094,
        help='SpireMS Core Port')
    args, unknown_args = parser.parse_known_args()
    if not os.path.isabs(args.config):
        current_path = os.path.abspath(__file__)
        params_dir = os.path.join(current_path[:current_path.find('spirecv-pro') + 11], 'params', 'spirecv2')
        args.config = os.path.join(params_dir, args.config)
    print("--config:", args.config)
    print("--job-name:", args.job_name)
    extra = get_extra_args(unknown_args)

    rclpy.init()
    node = ROS2ThreeLidar2SMSNode(args.job_name, param_dict_or_file=args.config, ip=args.ip, port=args.port, **extra)
    rclpy.spin(node)
    node.destroy_node()
    rclpy.shutdown()
